#ifndef _JOINT_
#define _JOINT_

#include <vector>
#include "BodyLink.h"
#include "Transformation.h"
#include "Dof.h"

using namespace std;

class BodyLink;
class Dof;

class Joint{
protected:
	BodyLink *mBodyIn;
	BodyLink *mBodyOut;
	int mModelID;	// unique to dof in model

	vector<Transformation *> mTransforms;	//transformations for mBodyOut
	vector<Dof *> mDofs;	// associated dofs
	void addDof(Dof *d);
public:
	enum Type {FREE, BALLEULER, BALLEXPMAP, HINGE, UNIVERSAL};
	
	Joint(BodyLink *_bIn, BodyLink *_bOut);
	~Joint(){
		for(int i=0; i<mTransforms.size(); i++){
			delete mTransforms[i];
		}
		mTransforms.clear();
		mDofs.clear();
	}
	void addTransform(Transformation *t, bool _isVariable=true);

	inline int getNumTransforms(){return static_cast<int>(mTransforms.size());}
	inline Transformation* getTransform(int i){return mTransforms[i];}
	inline int getNumDofs(){return static_cast<int>(mDofs.size());}
	inline Dof* getDof(int i){return mDofs[i];}
	inline BodyLink* getBodyIn(){return mBodyIn;}
	inline BodyLink* getBodyOut(){return mBodyOut;}
	inline void setModelID(int id){mModelID= id;}
};

#endif
